www.gusucode.com > C++ 图像分割源代码 > C++ 图像分割源代码/gusucode/seg_source/ImageSegmentation.cpp

    //Download by http://www.NewXing.com
//Copyright (c) 2004-2005, Baris Sumengen
//All rights reserved.
//
// CIMPL Matrix Performance Library
//
//Redistribution and use in source and binary
//forms, with or without modification, are
//permitted provided that the following
//conditions are met:
//
//    * No commercial use is allowed. 
//    This software can only be used
//    for non-commercial purposes. This 
//    distribution is mainly intended for
//    academic research and teaching.
//    * Redistributions of source code must
//    retain the above copyright notice, this
//    list of conditions and the following
//    disclaimer.
//    * Redistributions of binary form must
//    mention the above copyright notice, this
//    list of conditions and the following
//    disclaimer in a clearly visible part 
//    in associated product manual, 
//    readme, and web site of the redistributed 
//    software.
//    * Redistributions in binary form must
//    reproduce the above copyright notice,
//    this list of conditions and the
//    following disclaimer in the
//    documentation and/or other materials
//    provided with the distribution.
//    * The name of Baris Sumengen may not be
//    used to endorse or promote products
//    derived from this software without
//    specific prior written permission.
//
//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
//HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
//EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT
//NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//MERCHANTABILITY AND FITNESS FOR A PARTICULAR
//PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
//CONTRIBUTORS BE LIABLE FOR ANY
//DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//EXEMPLARY, OR CONSEQUENTIAL DAMAGES
//(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
//OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
//DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
//HOWEVER CAUSED AND ON ANY THEORY OF
//LIABILITY, WHETHER IN CONTRACT, STRICT
//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
//OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
//OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.


#include "./ImageSegmentation.h"
#include <queue>
#include <vector>





namespace ImageProcessing
{


// Gaussian Filter
	Matrix<float> Gaussian2D(int side, float sigma_x, float angle, float ratio)
	{
		Matrix<float> temp(2*side+1, 2*side+1); 

		float sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = 1.0/(2.0*PI*sigma_x*sigma_y)*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = (float)(sum_G/T);
			}
		}
		return temp;
	}



	Matrix<double> Gaussian2D(int side, double sigma_x, double angle, double ratio)
	{
		Matrix<double> temp(2*side+1, 2*side+1); 

		double sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = 1.0/(2.0*PI*sigma_x*sigma_y)*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = sum_G/T;
			}
		}
		return temp;
	}





// First Directional Derivative of Gaussian Filter


	Matrix<float> FDGaussian2D(int side, float sigma_x, float angle, float ratio)
	{
		Matrix<float> temp(2*side+1, 2*side+1);

		float sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = -x_new/(2.0*PI*sigma_x*sigma_y*sigma_x*sigma_x)*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = (float)(sum_G/T);
			}
		}
		return temp;
	}



	Matrix<double> FDGaussian2D(int side, double sigma_x, double angle, double ratio)
	{
		Matrix<double> temp(2*side+1, 2*side+1);

		double sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = -x_new/(2.0*PI*sigma_x*sigma_y*sigma_x*sigma_x)*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = sum_G/T;
			}
		}
		return temp;
	}


// Second Directional Derivative of Gaussian Filter


	Matrix<float> SDGaussian2D(int side, float sigma_x, float angle, float ratio)
	{
		Matrix<float> temp(2*side+1, 2*side+1);

		float sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);
		}
		else
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = (x_new*x_new-sigma_x*sigma_x)/(2.0*PI*sigma_x*sigma_y*sigma_x*sigma_x*sigma_x*sigma_x)*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = (float)(sum_G/T);
			}
		}
		return temp;
	}



	Matrix<double> SDGaussian2D(int side, double sigma_x, double angle, double ratio)
	{
		Matrix<double> temp(2*side+1, 2*side+1);

		double sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = (x_new*x_new-sigma_x*sigma_x)/(2.0*PI*sigma_x*sigma_y*sigma_x*sigma_x*sigma_x*sigma_x)*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = sum_G/T;
			}
		}
		return temp;
	}





// Laplacian of Gaussian Filter
	Matrix<float> LOG(int side, float sigma_x, float angle, float ratio)
	{
		Matrix<float> temp(2*side+1, 2*side+1); 

		float sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = (x_new*x_new+y_new*y_new-sigma_x*sigma_x-sigma_y*sigma_y)
							/(2.0*PI*sigma_x*sigma_y*(sigma_x*sigma_x*sigma_x*sigma_x+sigma_y*sigma_y*sigma_y*sigma_y))
							*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = (float)(sum_G/T);
			}
		}
		return temp;
	}



	Matrix<double> LOG(int side, double sigma_x, double angle, double ratio)
	{
		Matrix<double> temp(2*side+1, 2*side+1); 

		double sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = (x_new*x_new+y_new*y_new-sigma_x*sigma_x-sigma_y*sigma_y)
							/(2.0*PI*sigma_x*sigma_y*(sigma_x*sigma_x*sigma_x*sigma_x+sigma_y*sigma_y*sigma_y*sigma_y))
							*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = sum_G/T;
			}
		}
		return temp;
	}





// Difference of offset Gaussians filter
	Matrix<float> DOOG2D(int side, float sigma_x, float offset, float angle, float ratio)
	{
		Matrix<float> temp(2*side+1, 2*side+1);

		float sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0)
							- 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-((x_new-offset)*(x_new-offset)/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = (float)(sum_G/T);
			}
		}
		return temp;	
	}

	Matrix<double> DOOG2D(int side, double sigma_x, double offset, double angle, double ratio)
	{
		Matrix<double> temp(2*side+1, 2*side+1);

		double sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-(x_new*x_new/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0)
							- 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-((x_new-offset)*(x_new-offset)/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = sum_G/T;
			}
		}
		return temp;	
	}

// Difference of offset Gaussians filter (centered at the middle of two Gaussians)
	Matrix<float> DOOG2DCentered(int side, float sigma_x, float offset, float angle, float ratio)
	{
		Matrix<float> temp(2*side+1, 2*side+1);

		float sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);
		double half_offset = offset/2.0;

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-((x_new+half_offset)*(x_new+half_offset)/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0)
							- 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-((x_new-half_offset)*(x_new-half_offset)/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = (float)(sum_G/T);
			}
		}
		return temp;	
	}

	Matrix<double> DOOG2DCentered(int side, double sigma_x, double offset, double angle, double ratio)
	{
		Matrix<double> temp(2*side+1, 2*side+1);

		double sigma_y = sigma_x*ratio;

		double sin_angle = sin(angle*PI/180);
		double cos_angle = cos(angle*PI/180);
		double half_offset = offset/2.0;

		int sample;
		double d;
		if (sigma_x < 2.0 || sigma_y < 2.0) 
		{
			// use denser sample for better filter quality 
			sample = 5;
			d = 1.0/(2.0*sample+1.0);      
		}
		else 
		{
			sample = 0;
			d = 1.0;
		}

		double T = (double) (2*sample+1)*(2*sample+1);

		for (int i=0;i<2*side+1;i++) 
		{
			for (int j=0;j<2*side+1;j++) 
			{
				double sum_G = 0.0;
				double y = (double) (j-side)-d*sample-d;
				for (int sx=0;sx<2*sample+1;sx++) 
				{
					y += d;
					double x = (double) (i-side)-d*sample-d;
					for (int sy=0;sy<2*sample+1;sy++) 
					{
						x += d;

						double x_new = x*cos_angle + y*sin_angle;
						double y_new = -x*sin_angle + y*cos_angle;
						double g = 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-((x_new+half_offset)*(x_new+half_offset)/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0)
							- 1/(2.0*PI*sigma_x*sigma_y)
							*exp(-((x_new-half_offset)*(x_new-half_offset)/(sigma_x*sigma_x)+y_new*y_new/(sigma_y*sigma_y))/2.0);
						sum_G += g;
					}
				}

				temp.ElemNC(j,i) = sum_G/T;
			}
		}
		return temp;	
	}








// Filter image with these filters

	Matrix<float> FilterGaussian2D(Matrix<float>& image, float sigma_x, float angle, float ratio)
	{
		float sigma_y = ratio*sigma_x;
		float sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<float> filt = Gaussian2D(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}

	Matrix<double> FilterGaussian2D(Matrix<double>& image, double sigma_x, double angle, double ratio)
	{
		double sigma_y = ratio*sigma_x;
		double sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<double> filt = Gaussian2D(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}


	// First derivative of Gaussian
	Matrix<float> FilterFDGaussian2D(Matrix<float>& image, float sigma_x, float angle, float ratio)
	{
		float sigma_y = ratio*sigma_x;
		float sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<float> filt = FDGaussian2D(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}

	Matrix<double> FilterFDGaussian2D(Matrix<double>& image, double sigma_x, double angle, double ratio)
	{
		double sigma_y = ratio*sigma_x;
		double sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<double> filt = FDGaussian2D(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}


	// Second derivative of Gaussian
	Matrix<float> FilterSDGaussian2D(Matrix<float>& image, float sigma_x, float angle, float ratio)
	{
		float sigma_y = ratio*sigma_x;
		float sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<float> filt = SDGaussian2D(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}

	Matrix<double> FilterSDGaussian2D(Matrix<double>& image, double sigma_x, double angle, double ratio)
	{
		double sigma_y = ratio*sigma_x;
		double sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<double> filt = SDGaussian2D(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}


	// Laplacian of Gaussian
	Matrix<float> FilterLOG(Matrix<float>& image, float sigma_x, float angle, float ratio)
	{
		float sigma_y = ratio*sigma_x;
		float sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<float> filt = LOG(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}
	Matrix<double> FilterLOG(Matrix<double>& image, double sigma_x, double angle, double ratio)
	{
		double sigma_y = ratio*sigma_x;
		double sigma_max = sigma_x>=sigma_y?sigma_x:sigma_y;
		int side = (int)(3*sigma_max+0.5);
		Matrix<double> filt = LOG(side, sigma_x, angle, ratio);
		return Conv2(image, filt);
	}

	
	// Difference of offset Gaussians
	Matrix<float> FilterDOOG2D(Matrix<float>& image, float sigma_x, float offset, float angle, float ratio)
	{
		float sigma_y = ratio*sigma_x;
		int side = (int)((3*sigma_x+offset>=3*sigma_y?3*sigma_x+offset:3*sigma_y)+0.5);
		Matrix<float> filt = DOOG2D(side, sigma_x, offset, angle, ratio);
		return Conv2(image, filt);
	}

	Matrix<double> FilterDOOG2D(Matrix<double>& image, double sigma_x, double offset, double angle, double ratio)
	{
		double sigma_y = ratio*sigma_x;
		int side = (int)((3*sigma_x+offset>=3*sigma_y?3*sigma_x+offset:3*sigma_y)+0.5);
		Matrix<double> filt = DOOG2D(side, sigma_x, offset, angle, ratio);
		return Conv2(image, filt);
	}

	Matrix<float> FilterDOOG2DCentered(Matrix<float>& image, float sigma_x, float offset, float angle, float ratio)
	{
		float sigma_y = ratio*sigma_x;
		int side = (int)((3*sigma_x+offset/2>=3*sigma_y?3*sigma_x+offset/2:3*sigma_y)+0.5);
		Matrix<float> filt = DOOG2DCentered(side, sigma_x, offset, angle, ratio);
		return Conv2(image, filt);
	}

	Matrix<double> FilterDOOG2DCentered(Matrix<double>& image, double sigma_x, double offset, double angle, double ratio)
	{
		double sigma_y = ratio*sigma_x;
		int side = (int)((3*sigma_x+offset/2>=3*sigma_y?3*sigma_x+offset/2:3*sigma_y)+0.5);
		Matrix<double> filt = DOOG2DCentered(side, sigma_x, offset, angle, ratio);
		return Conv2(image, filt);
	}


// Various Edge Detection schemes
	Matrix<double> NonMaximaSuppress(Matrix<double>& edgesMain, Matrix<double>& theta)
	{
		Matrix<double> vectorX = Cos(theta);
		Matrix<double> vectorY = Sin(theta);
		return NonMaximaSuppress(edgesMain, vectorX, vectorY);
	}

	Matrix<float> NonMaximaSuppress(Matrix<float>& edgesMain, Matrix<float>& theta)
	{
		Matrix<float> vectorX = Cos(theta);
		Matrix<float> vectorY = Sin(theta);
		return NonMaximaSuppress(edgesMain, vectorX, vectorY);
	}


	Matrix<double> NonMaximaSuppress(Matrix<double>& edgesMain, Matrix<double>& vectorX, Matrix<double>& vectorY)
	{
		Matrix<double> edges = edgesMain.Clone();
		Matrix<int> mask = NonMaximaMask(edges - Minimum(edges(":")), vectorX, vectorY);

		for(int i=0;i<edges.Columns();i++)
		{
			for(int j=0;j<edges.Rows();j++)
			{
				if(mask(j,i) == 0)
				{
					edges(j,i) = 0;
				}
			}
		}

		return edges;
	}

	Matrix<float> NonMaximaSuppress(Matrix<float>& edgesMain, Matrix<float>& vectorX, Matrix<float>& vectorY)
	{
		Matrix<float> edges = edgesMain.Clone();
		Matrix<int> mask = NonMaximaMask(edges - Minimum(edges(":")), vectorX, vectorY);

		for(int i=0;i<edges.Columns();i++)
		{
			for(int j=0;j<edges.Rows();j++)
			{
				if(mask(j,i) == 0)
				{
					edges(j,i) = 0;
				}
			}
		}

		return edges;
	}


	Matrix<int> NonMaximaMask(Matrix<double>& edges, Matrix<double>& vectorX, Matrix<double>& vectorY)
	{
		Matrix<int> mask(edges.Rows(), edges.Columns(), 1);
		Matrix<double> theta(edges.Rows(), edges.Columns(), 0);
		
		Matrix<int> mask15(edges.Rows(), edges.Columns(), 0);
		Matrix<int> mask26(edges.Rows(), edges.Columns(), 0);
		Matrix<int> mask37(edges.Rows(), edges.Columns(), 0);
		Matrix<int> mask48(edges.Rows(), edges.Columns(), 0);

		for(int i=0;i<theta.Columns();i++)
		{
			for(int j=0;j<theta.Rows();j++)
			{
				theta(j,i) = Direction(vectorY(j,i), vectorX(j,i));

				if( theta(j,i) >= 0 && theta(j,i) < PI/4 )
				{
					mask15(j,i) = 1;
				}

				else if( theta(j,i) >= PI/4 && theta(j,i) < PI/2 )
				{
					mask26(j,i) = 1;
				}

				else if( theta(j,i) >= PI/2 && theta(j,i) < PI*3/4 )
				{
					mask37(j,i) = 1;
				}

				else if( theta(j,i) >= PI*3/4 && theta(j,i) < PI )
				{
					mask48(j,i) = 1;
				}

			}

		}



		//Case 1
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask15(j,i) == 1)
				{
					double d = tan(theta(j,i));
					double interpolated = edges(j,i+1)*(1-d) + edges(j+1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}

		//Case 5
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask15(j,i) == 1)
				{
					double d = tan(theta(j,i));
					double interpolated = edges(j,i-1)*(1-d) + edges(j-1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 2
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask26(j,i) == 1)
				{
					double d = tan(PI/2 - theta(j,i));
					double interpolated = edges(j+1,i)*(1-d) + edges(j+1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}

		//Case 6
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask26(j,i) == 1)
				{
					double d = tan(PI/2 - theta(j,i));
					double interpolated = edges(j-1,i)*(1-d) + edges(j-1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}




		//Case 3
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask37(j,i) == 1)
				{
					double d = tan(theta(j,i) - PI/2);
					double interpolated = edges(j+1,i)*(1-d) + edges(j+1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 7
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask37(j,i) == 1)
				{
					double d = tan(theta(j,i) - PI/2);
					double interpolated = edges(j-1,i)*(1-d) + edges(j-1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 4
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask48(j,i) == 1)
				{
					double d = tan(PI - theta(j,i));
					double interpolated = edges(j,i-1)*(1-d) + edges(j+1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 8
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask48(j,i) == 1)
				{
					double d = tan(PI - theta(j,i));
					double interpolated = edges(j,i+1)*(1-d) + edges(j-1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}

		return mask;

	}


	Matrix<int> NonMaximaMask(Matrix<float>& edges, Matrix<float>& vectorX, Matrix<float>& vectorY)
	{
		Matrix<int> mask(edges.Rows(), edges.Columns(), 1);
		Matrix<double> theta(edges.Rows(), edges.Columns(), 0);
		
		Matrix<int> mask15(edges.Rows(), edges.Columns(), 0);
		Matrix<int> mask26(edges.Rows(), edges.Columns(), 0);
		Matrix<int> mask37(edges.Rows(), edges.Columns(), 0);
		Matrix<int> mask48(edges.Rows(), edges.Columns(), 0);

		for(int i=0;i<theta.Columns();i++)
		{
			for(int j=0;j<theta.Rows();j++)
			{
				theta(j,i) = Direction(vectorY(j,i), vectorX(j,i));

				if( theta(j,i) >= 0 && theta(j,i) < PI/4 )
				{
					mask15(j,i) = 1;
				}

				else if( theta(j,i) >= PI/4 && theta(j,i) < PI/2 )
				{
					mask26(j,i) = 1;
				}

				else if( theta(j,i) >= PI/2 && theta(j,i) < PI*3/4 )
				{
					mask37(j,i) = 1;
				}

				else if( theta(j,i) >= PI*3/4 && theta(j,i) < PI )
				{
					mask48(j,i) = 1;
				}

			}

		}



		//Case 1
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask15(j,i) == 1)
				{
					double d = tan(theta(j,i));
					double interpolated = edges(j,i+1)*(1-d) + edges(j+1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}

		//Case 5
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask15(j,i) == 1)
				{
					double d = tan(theta(j,i));
					double interpolated = edges(j,i-1)*(1-d) + edges(j-1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 2
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask26(j,i) == 1)
				{
					double d = tan(PI/2 - theta(j,i));
					double interpolated = edges(j+1,i)*(1-d) + edges(j+1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}

		//Case 6
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask26(j,i) == 1)
				{
					double d = tan(PI/2 - theta(j,i));
					double interpolated = edges(j-1,i)*(1-d) + edges(j-1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}




		//Case 3
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask37(j,i) == 1)
				{
					double d = tan(theta(j,i) - PI/2);
					double interpolated = edges(j+1,i)*(1-d) + edges(j+1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 7
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask37(j,i) == 1)
				{
					double d = tan(theta(j,i) - PI/2);
					double interpolated = edges(j-1,i)*(1-d) + edges(j-1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 4
		for(int i=1;i<theta.Columns();i++)
		{
			for(int j=0;j<theta.Rows()-1;j++)
			{
				if(mask48(j,i) == 1)
				{
					double d = tan(PI - theta(j,i));
					double interpolated = edges(j,i-1)*(1-d) + edges(j+1,i-1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}


		//Case 8
		for(int i=0;i<theta.Columns()-1;i++)
		{
			for(int j=1;j<theta.Rows();j++)
			{
				if(mask48(j,i) == 1)
				{
					double d = tan(PI - theta(j,i));
					double interpolated = edges(j,i+1)*(1-d) + edges(j-1,i+1)*d;
					if(edges(j,i)<interpolated)
					{
						mask(j,i) = 0;
					}
				}
			}
		}

		return mask;

	}






	double Direction(double y, double x)
	{
		if(x>=0 && y>=0)
		{
			return atan2(y,x);
		}
		else if(x<0 && y == 0)
		{
			return atan2(y,x) - PI;
		}
		else if(x<0 && y>0)
		{
			return atan2(y,x);
		}
		else if(x<=0 && y<0)
		{
			return PI + atan2(y,x) ;
		}
		else if(x>0 && y<0)
		{
			return PI + atan2(y,x) ;
		}
		else 
		{
			cout << "Something went wrong with Direction(x,y) function.. Returning -1..\n" << endl;
			return -1;
		}
	}

	float Direction(float y, float x)
	{
		if(x>=0 && y>=0)
		{
			return atan2(y,x);
		}
		else if(x<0 && y == 0)
		{
			return atan2(y,x) - PI;
		}
		else if(x<0 && y>0)
		{
			return atan2(y,x);
		}
		else if(x<=0 && y<0)
		{
			return PI + atan2(y,x) ;
		}
		else if(x>0 && y<0)
		{
			return PI + atan2(y,x) ;
		}
		else 
		{
			cout << "Something went wrong with Direction(x,y) function.. Returning -1..\n" << endl;
			return -1;
		}
	}



// Edgeflow

// both grayscale and multi-valued
	// Outout is two dimensional (x and y components of the vector field)
	MatrixList<float> EdgeflowVectorField(Matrix<float>& image, int angles, float sigma, float offset, float ratio, bool normalized)
	{

		Vector<float> radAngles(2*angles);
		Vector<float> cosAngles(2*angles);
		Vector<float> sinAngles(2*angles);
		for(int i=0; i<angles; i++)
		{
			radAngles[i] = (float)(i*PI/angles);
			cosAngles[i] = cos(radAngles[i]);
			sinAngles[i] = sin(radAngles[i]);

			radAngles[i+angles] = (float)((i+angles)*PI/angles);
			cosAngles[i+angles] = cos(radAngles[i+angles]);
			sinAngles[i+angles] = sin(radAngles[i+angles]);

		}

		// Find energies at directions theta and theta+pi
		Vector<Matrix<float> > energies(2*angles);
		float sigma_y = ratio*sigma;
		int side = (int)((3*sigma+offset>=3*sigma_y?3*sigma+offset:3*sigma_y)+0.5);
		for(int i=0; i<angles; i++)
		{
			float angle = (float)(i*(180.0/angles));
			
			//char s[200];
			Matrix<float> filt = DOOG2D(side, sigma, offset, 180+angle, ratio);
			energies(i) = AbsI(Conv2(image, filt));
			//sprintf(s, "%02d.0.bmp", i);
			//ImWrite(energies(i),s);
			filt = DOOG2D(side, sigma, offset, angle, ratio);
			energies(i+angles) = AbsI(Conv2(image, filt));
			//sprintf(s, "%02d.1.bmp", i);
			//ImWrite(energies(i+angles),s);

			//Matrix<float> filt = DOOG2DCentered(side, sigma, offset, angle, ratio);
			//char s[200];
			//Matrix<float> filtOut = Conv2(image, filt, Full);
			//
			//int offsetX = (int)(offset*cos(radAngles[i])/2+0.5);
			//offsetX = offsetX==0?1:offsetX;
			//int offsetY = (int)(offset*sin(radAngles[i])/2+0.5);
			//offsetY = offsetY==0?1:offsetY;
			//// cout << (fmod(2.0+cos((double)radAngles[i]),2.0)-1) << " : " << (fmod(2.0+sin((double)radAngles[i]),2.0)-1) << endl;

			//energies(i) = Abs(filtOut.Slice(side+offsetY, image.Rows()+side+offsetY-1, side+offsetX, image.Columns()+side+offsetX-1));
			//sprintf(s, "%02d.0.bmp", i);
			//ImWrite(energies(i), s);
			//
			//energies(i+angles) = Abs(filtOut.Slice(side-offsetY, image.Rows()+side-offsetY-1, side-offsetX, image.Columns()+side-offsetX-1));
			//sprintf(s, "%02d.1.bmp", i);
			//ImWrite(energies(i+angles), s);
		}
		//pf.Toc();

		// Sum energies over half circles
		//pf.Tic();
		Vector<Matrix<float> > sumEnergies(2*angles);
		for(int i=0; i<2*angles; i++)
		{
			sumEnergies(i) = Matrix<float>(image.Rows(), image.Columns(), 0.0f);
			int start = i-angles/2;
			int end = start+angles;
			for(int j=start; j<end; j++)
			{
				int ind = (j+2*angles)%(2*angles);
				sumEnergies(i) += energies(ind);
			}
		}
		//pf.Toc();

		// normalize summed energies to make them like probabilities.
		//pf.Tic();
		Vector<Matrix<float> > probabilities(2*angles);
		for(int i=0; i<angles; i++)
		{
			probabilities(i) = Matrix<float>(image.Rows(), image.Columns());
			probabilities(i+angles) = Matrix<float>(image.Rows(), image.Columns());
			Matrix<float> total = sumEnergies(i) + sumEnergies(i+angles);
			for(int r=0;r<image.Rows();r++)
			{
				for(int c=0;c<image.Columns();c++)
				{
					if(total(r,c) > 1e-9)
					{
						probabilities(i).Elem(r,c) = sumEnergies(i).Elem(r,c)/total(r,c);
						probabilities(i+angles).Elem(r,c) = sumEnergies(i+angles).Elem(r,c)/total(r,c);
					}
					else
					{
						probabilities(i).Elem(r,c) = 0.5;
						probabilities(i+angles).Elem(r,c) = 0.5;
					}
				}
			}
		}
		//pf.Toc();

		//pf.Tic();
		Matrix<float> xFlow(image.Rows(), image.Columns());
		Matrix<float> yFlow(image.Rows(), image.Columns());
		Matrix<float> maxEnergy(image.Rows(), image.Columns());
		for(int r=0;r<image.Rows();r++)
		{
			for(int c=0;c<image.Columns();c++)
			{

				float dirX = 0;
				float dirY = 0;
				//int count = 0;
				for(int k=0;k<2*angles;k++)
				{
					float v = probabilities(k).Elem(r,c);
					float vl = probabilities((k+2*angles-1)%(2*angles)).Elem(r,c);
					float vr = probabilities((k+2*angles+1)%(2*angles)).Elem(r,c);
					if(v>=vl && v>=vr)
					{
						float orientation, strength;
						if(vl+vr != 2*v)
						{
							orientation = 0.5f*(vl - vr)/(vl + vr - 2*v);
							strength = v + 0.5f*( (vr - vl)*orientation 
								+ (vr + vl - 2*v)*orientation*orientation );
							orientation = (float)((k+orientation)*PI/angles);
						}
						else
						{
							strength = v;
							orientation = (float)(k*PI/angles);
						}
						dirX += cos(orientation)*strength;
						dirY += sin(orientation)*strength;
						//maxEnergy(r,c) += strength;
						//count++;
					}
				}

				//maxEnergy(r,c) /= (float)count;

				float dir = sqrt(dirX*dirX+dirY*dirY);
				dirX /= dir;
				dirY /= dir;


				float value;
				int ang;
				if(probabilities(0).Elem(r,c)>probabilities(angles).Elem(r,c))
				{
					value = probabilities(0).Elem(r,c);
					ang = 0;
				}
				else
				{
					value = probabilities(angles).Elem(r,c);
					ang = angles;
				}


				float maximum = value;
				int maxIndex = ang;
				float minimum = value;
				int minIndex = ang;
				int maxOrientation = 0;
				int minOrientation = 0;

				for(int k=1;k<angles;k++)
				{
					if(probabilities(k).Elem(r,c)>probabilities(k+angles).Elem(r,c))
					{
						value = probabilities(k).Elem(r,c);
						ang = k;
					}
					else
					{
						value = probabilities(k+angles).Elem(r,c);
						ang = k+angles;
					}

					if(value > maximum)
					{
						maximum = value;
						maxIndex = ang;
						maxOrientation = k;
					}
					if(value < minimum)
					{
						minimum = value;
						minIndex = ang;
						minOrientation = k;
					}
				}
				if(normalized)
				{
					xFlow(r,c) = dirX*probabilities(maxIndex)(r,c);
					yFlow(r,c) = dirY*probabilities(maxIndex)(r,c);
				}
				else
				{
					xFlow(r,c) = dirX*sumEnergies(maxIndex)(r,c);
					yFlow(r,c) = dirY*sumEnergies(maxIndex)(r,c);
				}
				maxEnergy(r,c) = sumEnergies(maxIndex)(r,c);
			}
		}
		//if(normalized)
		//{
		//	xFlow *= ToFloat(maxEnergy > (float)angles);
		//	yFlow *= ToFloat(maxEnergy > (float)angles);
		//}

		//ImWrite(xFlow, "xflow.bmp");
		//ImWrite(yFlow, "yflow.bmp");
		//ImWrite(maxEnergy, "maxEnergy.bmp");
		return MatrixList<float>(xFlow, yFlow);

	}

	//MatrixList<double> EdgeflowVectorField(Matrix<double> image, double sigma, double offset, double ratio, int angles)
	//{
	//}

	MatrixList<float> EdgeflowVectorField(MatrixList<float>& image, int angles, float sigma, float offset, float ratio, bool normalized)
	{

		Vector<float> radAngles(2*angles);
		Vector<float> cosAngles(2*angles);
		Vector<float> sinAngles(2*angles);
		for(int i=0; i<angles; i++)
		{
			radAngles[i] = (float)(i*PI/angles);
			cosAngles[i] = cos(radAngles[i]);
			sinAngles[i] = sin(radAngles[i]);

			radAngles[i+angles] = (float)((i+angles)*PI/angles);
			cosAngles[i+angles] = cos(radAngles[i+angles]);
			sinAngles[i+angles] = sin(radAngles[i+angles]);

		}

		// Find energies at directions theta and theta+pi
		Vector<Matrix<float> > energies(2*angles);
		for(int i=0;i<energies.Length();i++)
		{
			energies(i) = Matrix<float>(image.Rows(), image.Columns(), 0);
		}
		float sigma_y = ratio*sigma;
		int side = (int)((3*sigma+offset>=3*sigma_y?3*sigma+offset:3*sigma_y)+0.5);
		for(int i=0; i<angles; i++)
		{
			float angle = (float)(i*(180.0/angles));
			for(int c=0; c<image.Planes();c++)
			{
				//char s[200];
				Matrix<float> filt = DOOG2D(side, sigma, offset, 180+angle, ratio);
				energies(i) += AbsI(Conv2(image[c], filt));
				//sprintf(s, "%02d.0.bmp", i);
				//ImWrite(energies(i),s);
				filt = DOOG2D(side, sigma, offset, angle, ratio);
				energies(i+angles) += AbsI(Conv2(image[c], filt));
				//sprintf(s, "%02d.1.bmp", i);
				//ImWrite(energies(i+angles),s);
			}
			//Matrix<float> filt = DOOG2DCentered(side, sigma, offset, angle, ratio);
			//char s[200];
			//Matrix<float> filtOut = Conv2(image, filt, Full);
			//
			//int offsetX = (int)(offset*cos(radAngles[i])/2+0.5);
			//offsetX = offsetX==0?1:offsetX;
			//int offsetY = (int)(offset*sin(radAngles[i])/2+0.5);
			//offsetY = offsetY==0?1:offsetY;
			//// cout << (fmod(2.0+cos((double)radAngles[i]),2.0)-1) << " : " << (fmod(2.0+sin((double)radAngles[i]),2.0)-1) << endl;

			//energies(i) = Abs(filtOut.Slice(side+offsetY, image.Rows()+side+offsetY-1, side+offsetX, image.Columns()+side+offsetX-1));
			//sprintf(s, "%02d.0.bmp", i);
			//ImWrite(energies(i), s);
			//
			//energies(i+angles) = Abs(filtOut.Slice(side-offsetY, image.Rows()+side-offsetY-1, side-offsetX, image.Columns()+side-offsetX-1));
			//sprintf(s, "%02d.1.bmp", i);
			//ImWrite(energies(i+angles), s);
		}
		//pf.Toc();

		// Sum energies over half circles
		//pf.Tic();
		Vector<Matrix<float> > sumEnergies(2*angles);
		for(int i=0; i<2*angles; i++)
		{
			sumEnergies(i) = Matrix<float>(image.Rows(), image.Columns(), 0.0f);
			int start = i-angles/2;
			int end = start+angles;
			for(int j=start; j<end; j++)
			{
				int ind = (j+2*angles)%(2*angles);
				sumEnergies(i) += energies(ind);
			}
		}
		//pf.Toc();

		// normalize summed energies to make them like probabilities.
		//pf.Tic();
		Vector<Matrix<float> > probabilities(2*angles);
		for(int i=0; i<angles; i++)
		{
			probabilities(i) = Matrix<float>(image.Rows(), image.Columns());
			probabilities(i+angles) = Matrix<float>(image.Rows(), image.Columns());
			Matrix<float> total = sumEnergies(i) + sumEnergies(i+angles);
			for(int r=0;r<image.Rows();r++)
			{
				for(int c=0;c<image.Columns();c++)
				{
					if(total(r,c) > 1e-9)
					{
						probabilities(i).Elem(r,c) = sumEnergies(i).Elem(r,c)/total(r,c);
						probabilities(i+angles).Elem(r,c) = sumEnergies(i+angles).Elem(r,c)/total(r,c);
					}
					else
					{
						probabilities(i).Elem(r,c) = 0.5;
						probabilities(i+angles).Elem(r,c) = 0.5;
					}
				}
			}
		}
		//pf.Toc();

		//pf.Tic();
		Matrix<float> xFlow(image.Rows(), image.Columns());
		Matrix<float> yFlow(image.Rows(), image.Columns());
		Matrix<float> maxEnergy(image.Rows(), image.Columns());
		for(int r=0;r<image.Rows();r++)
		{
			for(int c=0;c<image.Columns();c++)
			{

				float dirX = 0;
				float dirY = 0;
				//int count = 0;
				for(int k=0;k<2*angles;k++)
				{
					float v = probabilities(k).Elem(r,c);
					float vl = probabilities((k+2*angles-1)%(2*angles)).Elem(r,c);
					float vr = probabilities((k+2*angles+1)%(2*angles)).Elem(r,c);
					if(v>=vl && v>=vr)
					{
						float orientation, strength;
						if(vl+vr != 2*v)
						{
							orientation = 0.5f*(vl - vr)/(vl + vr - 2*v);
							strength = v + 0.5f*( (vr - vl)*orientation 
								+ (vr + vl - 2*v)*orientation*orientation );
							orientation = (float)((k+orientation)*PI/angles);
						}
						else
						{
							strength = v;
							orientation = (float)(k*PI/angles);
						}
						dirX += cos(orientation)*strength;
						dirY += sin(orientation)*strength;
						//maxEnergy(r,c) += strength;
						//count++;
					}
				}

				//maxEnergy(r,c) /= (float)count;

				float dir = sqrt(dirX*dirX+dirY*dirY);
				dirX /= dir;
				dirY /= dir;


				float value;
				int ang;
				if(probabilities(0).Elem(r,c)>probabilities(angles).Elem(r,c))
				{
					value = probabilities(0).Elem(r,c);
					ang = 0;
				}
				else
				{
					value = probabilities(angles).Elem(r,c);
					ang = angles;
				}


				float maximum = value;
				int maxIndex = ang;
				float minimum = value;
				int minIndex = ang;
				int maxOrientation = 0;
				int minOrientation = 0;

				for(int k=1;k<angles;k++)
				{
					if(probabilities(k).Elem(r,c)>probabilities(k+angles).Elem(r,c))
					{
						value = probabilities(k).Elem(r,c);
						ang = k;
					}
					else
					{
						value = probabilities(k+angles).Elem(r,c);
						ang = k+angles;
					}

					if(value > maximum)
					{
						maximum = value;
						maxIndex = ang;
						maxOrientation = k;
					}
					if(value < minimum)
					{
						minimum = value;
						minIndex = ang;
						minOrientation = k;
					}
				}
				if(normalized)
				{
					xFlow(r,c) = dirX*probabilities(maxIndex)(r,c);
					yFlow(r,c) = dirY*probabilities(maxIndex)(r,c);
				}
				else
				{
					xFlow(r,c) = dirX*sumEnergies(maxIndex)(r,c);
					yFlow(r,c) = dirY*sumEnergies(maxIndex)(r,c);
				}
				maxEnergy(r,c) = sumEnergies(maxIndex)(r,c);
			}
		}
		//if(normalized)
		//{
		//	xFlow *= ToFloat(maxEnergy > (float)angles);
		//	yFlow *= ToFloat(maxEnergy > (float)angles);
		//}

		//ImWrite(xFlow, "xflow.bmp");
		//ImWrite(yFlow, "yflow.bmp");
		//ImWrite(maxEnergy, "maxEnergy.bmp");
		return MatrixList<float>(xFlow, yFlow);

	}



	void block_write(Matrix<int>& matrix, int x_ind, int y_ind, int *keys, int energy);
	double my_atan2(double y, double x);

	Matrix<int> CreateFlowImage(Matrix<float>& xFlow, Matrix<float>& yFlow)
	{
		if(xFlow.Rows() != yFlow.Rows() || xFlow.Columns() != yFlow.Columns())
		{
			cerr << "Line: " << __LINE__ << " File: " << __FILE__ << endl;
			Utility::RunTimeError("Matrix dimensions does not match to each other!");
		}

		int display_map[8][15] ={37,38,39,40,41,42,43,33,23,13,12,51,59,67,66,
								10,20,30,40,50,60,70,61,52,43,34,69,68,67,66,
								13,22,31,40,49,58,67,59,51,43,34,57,47,37,28,
								16,24,32,40,48,56,64,55,46,37,28,65,66,67,68,
								43,42,41,40,39,38,37,47,57,67,68,29,21,13,14,
								70,60,50,40,30,20,10,19,28,37,46,11,12,13,14,
								67,58,49,40,31,22,13,21,29,37,46,23,33,43,52,
								64,56,48,40,32,24,16,25,34,43,52,15,14,13,12};
								
		int quant[17] = {0, 45, 45, 90, 90, 135, 135, 180, 180, 225, 225, 270, 270, 315, 315, 0, 0};

		Matrix<int> angles(xFlow.Rows(), xFlow.Columns());
		Matrix<float> raw_angles(xFlow.Rows(), xFlow.Columns());
		Matrix<int> flow(9*xFlow.Rows(), 9*xFlow.Columns(), 255);

		Matrix<float> energies = SqrtI(xFlow*xFlow + yFlow*yFlow);
		float maximum = Maximum(energies(":"));
		float minimum = Minimum(energies(":"));
		Matrix<int> scaled_energies(xFlow.Rows(), xFlow.Columns(), 0);
		if(maximum-minimum > 1e-9)
		{
			scaled_energies = 255 - ToInt((energies-minimum)*255.0f/(maximum-minimum));
			//scaled_energies = 255 - ToInt(energies*255.0f/maximum);
		}

		int disp_keys[15];
		for(int i=0;i<xFlow.Columns();i++){
			for(int j=0;j<xFlow.Rows();j++){
				float angle = (float)my_atan2( yFlow(j,i), xFlow(j,i) ) ;
				raw_angles(j,i) = angle;
				int q_angle = (int)(angle/22.5);
				q_angle = (q_angle+17)%17;
				angles(j,i) =  quant[q_angle];
				int l = quant[q_angle] / 45;
				for(int s=0;s<15;s++){
					disp_keys[s] = display_map[l][s];
				}
				int q_energy = scaled_energies(j,i);
				block_write(flow, i, j, disp_keys, q_energy);
			}
		}

		return flow;
	}

	void block_write(Matrix<int>& matrix, int x_ind, int y_ind, int *keys, int energy){
		for(int t=0;t<15;t++){
			int x_loc = 9*x_ind + (keys[t] % 9);
			int y_loc = 9*y_ind + (keys[t] / 9);
			matrix(y_loc,x_loc) = energy;
		}
	}

	double my_atan2(double y, double x){
		if(x>=0 && y>=0){
			return atan2(y,x)*180.0/PI;
		}
		else if(x>=0 && y<=0){
			return ( 2*PI + atan2(y,x) )*180.0/PI;
		}
		else if(x<=0 && y>=0){
			return atan2(y,x)*180.0/PI;
		}
		else if(x<=0 && y<=0){
			return (2*PI + atan2(y,x) )*180.0/PI;
		}
		else
		{
			return -1;
		}
	}



	Matrix<int> HystThreshold(Matrix<float>& m, float thHigh, float thLow)
	{
		Matrix<int> th = (m > thHigh);
		Matrix<int> th2 = (m > thLow);

		int count;
		do{
			count = 0;
			for(int i=0;i<m.Rows();i++)
			{
				for(int j=0;j<m.Columns();j++)
				{
					if(th2.ElemNC(i,j) == 0 || th.ElemNC(i,j) == 1)
					{
						continue;
					}

					if(i>0 && th.ElemNC(i-1,j)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}
					if(i<m.Rows()-1 && th.ElemNC(i+1,j)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}
					if(j>0 && th.ElemNC(i,j-1)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}
					if(j<m.Columns()-1 && th.ElemNC(i,j+1)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}

					if(i>0 && j>0 && th.ElemNC(i-1,j-1)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}
					if(i<m.Rows()-1 && j>0 && th.ElemNC(i+1,j-1)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}
					if(i> 0 && j<m.Columns()-1 && th.ElemNC(i-1,j+1)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}
					if(i<m.Rows()-1 && j<m.Columns()-1 && th.ElemNC(i+1,j+1)==1)
					{
						th.ElemNC(i,j) = 1;
						count++;
					}

				}
			}
			//cout << count << endl;
		}while(count > 0);
		
		return th;

	}







	Matrix<float>& PMAnisoDiff(Matrix<float>& image, float K, int iterations)
	{
		float lambda = 0.25;
		float K2Inv = 1/(K*K);
		Matrix<float> gradX(image.Rows(), image.Columns());
		Matrix<float> gradY(image.Rows(), image.Columns());
		gradX(0, image.Rows()-1, 0, 0) = 0;
		gradY(0, 0, 0, image.Columns()-1) = 0;

		for(int k=0; k<iterations; k++)
		{
			// Calculate gradient
			for(int i=0;i<image.Rows();i++)
			{
				for(int j=1;j<image.Columns();j++)
				{
					gradX.ElemNC(i,j) = image.ElemNC(i,j-1) - image.ElemNC(i,j);
				}
			}

			for(int i=1;i<image.Rows();i++)
			{
				for(int j=0;j<image.Columns();j++)
				{
					gradY.ElemNC(i,j) = image.ElemNC(i-1,j) - image.ElemNC(i,j);
				}
			}


			// Calculate flux
			Matrix<float> fluxX = gradX*Exp(-K2Inv*Abs(gradX));
			Matrix<float> fluxY = gradY*Exp(-K2Inv*Abs(gradY));


			// Update image
			for(int i=0;i<image.Rows()-1;i++)
			{
				for(int j=0;j<image.Columns()-1;j++)
				{
					image.ElemNC(i,j) += lambda*(fluxX.ElemNC(i,j) - fluxX.ElemNC(i,j+1) 
						+ fluxY.ElemNC(i,j) - fluxY.ElemNC(i+1,j) );
				}
			}


			for(int j=0;j<image.Rows()-1;j++)
			{
				image.ElemNC(j, image.Columns()-1) += lambda*(fluxY.ElemNC(j, image.Columns()-1) 
					- fluxY.ElemNC(j+1, image.Columns()-1) );
			}

			for(int i=0;i<image.Columns()-1;i++)
			{
				image.ElemNC(image.Rows()-1, i) += lambda*(fluxX.ElemNC(image.Rows()-1, i) 
					- fluxX.ElemNC(image.Rows()-1, i+1) );
			}
		}
		return image;
	}

	Matrix<double>& PMAnisoDiff(Matrix<double>& image, double K, int iterations)
	{
		double lambda = 0.25;
		double K2Inv = 1/(K*K);
		Matrix<double> gradX(image.Rows(), image.Columns());
		Matrix<double> gradY(image.Rows(), image.Columns());
		gradX(0, image.Rows()-1, 0, 0) = 0;
		gradY(0, 0, 0, image.Columns()-1) = 0;

		for(int k=0; k<iterations; k++)
		{
			// Calculate gradient
			for(int i=0;i<image.Rows();i++)
			{
				for(int j=1;j<image.Columns();j++)
				{
					gradX.ElemNC(i,j) = image.ElemNC(i,j-1) - image.ElemNC(i,j);
				}
			}

			for(int i=1;i<image.Rows();i++)
			{
				for(int j=0;j<image.Columns();j++)
				{
					gradY.ElemNC(i,j) = image.ElemNC(i-1,j) - image.ElemNC(i,j);
				}
			}


			// Calculate flux
			Matrix<double> fluxX = gradX*Exp(-K2Inv*Abs(gradX));
			Matrix<double> fluxY = gradY*Exp(-K2Inv*Abs(gradY));


			// Update image
			for(int i=0;i<image.Rows()-1;i++)
			{
				for(int j=0;j<image.Columns()-1;j++)
				{
					image.ElemNC(i,j) += lambda*(fluxX.ElemNC(i,j) - fluxX.ElemNC(i,j+1) 
						+ fluxY.ElemNC(i,j) - fluxY.ElemNC(i+1,j) );
				}
			}


			for(int j=0;j<image.Rows()-1;j++)
			{
				image.ElemNC(j, image.Columns()-1) += lambda*(fluxY.ElemNC(j, image.Columns()-1) 
					- fluxY.ElemNC(j+1, image.Columns()-1) );
			}

			for(int i=0;i<image.Columns()-1;i++)
			{
				image.ElemNC(image.Rows()-1, i) += lambda*(fluxX.ElemNC(image.Rows()-1, i) 
					- fluxX.ElemNC(image.Rows()-1, i+1) );
			}
		}
		return image;
	}

	//MatrixList<float>& PMAnisoDiff(MatrixList<float>& image, float K, int iterations)
	//{
	//}

	//MatrixList<double>& PMAnisoDiff(MatrixList<double>& image, double K, int iterations)
	//{
	//}





	class CompareGrads
	{
	public:
		CompareGrads(){}
		~CompareGrads(){}

		static Matrix<float> gradMatrix;
		static int Compare( Point px, Point py )  
		{
			if(gradMatrix(px.y,px.x) > gradMatrix(py.y,py.x))
			{
				return 1;
			}
			else if(gradMatrix(px.y,px.x) == gradMatrix(py.y,py.x))
			{
				return 0;
			}
			else
			{
				return -1;
			}
		}

	};


	Matrix<float> CompareGrads::gradMatrix;

	Matrix<int> GetEgdes(Matrix<float> &grads, Matrix<int> &thick, Matrix<int> &suppressed)
	{
		Matrix<int> edges(grads.Rows(), grads.Columns(), 0);

		Vector<int> LabelCount(grads.Rows()*grads.Columns(), 0);

		Vector<float> LabelAvg(grads.Rows()*grads.Columns(), 0.0f);
		
		Matrix<int> labels(grads.Rows(), grads.Columns(), 0);

		Matrix<int> dummy(grads.Rows(), grads.Columns(), 0);

		int label = 0;


		// Point struct needs to be defined.

		queue<Point> outsQ;
		queue<Point> tmpQ;


		for (int x=0; x<grads.Columns(); x++) 
		{
			for (int y=0; y<grads.Rows(); y++) 
			{
				if(dummy(y,x) == 0 && thick(y,x) == 0)
				{
					label++;
					tmpQ.push(Point(x,y));
					dummy(y,x) = 1;
					labels(y,x) = label;
					LabelCount(label)++;
					LabelAvg(label) += grads(y,x);

					while(tmpQ.size() > 0)
					{
						Point p = tmpQ.front();
						tmpQ.pop();

						for (int i=-1; i<=1; i++) 
						{
							for (int j=-1; j<=1; j++) 
							{
								if(i == 0 || j == 0) 
								{
									if(p.x+i>=0 && p.x+i<grads.Columns() && p.y+j>=0 && p.y+j<grads.Rows()) 
									{
										if(dummy(p.y+j,p.x+i) == 0 && thick(p.y+j,p.x+i) == 0)
										{
											tmpQ.push(Point(p.x+i,p.y+j));
											dummy(p.y+j,p.x+i) = 1;
											labels(p.y+j,p.x+i) = label;
											LabelCount(label)++;
											LabelAvg(label) += grads(p.y+j,p.x+i);
										}
									}
								}
							}
						}
					}

				}
				else if(thick(y,x) == 1)
				{
					outsQ.push(Point(x,y));
				}
			}
		}


		for(int i=1;i<=label;i++)
		{
			LabelAvg(i) /= LabelCount(i);
			//					Console.WriteLine(i + ": " + LabelCount[i] + " : " + LabelAvg[i]);
		}
		




		// Utilize suppressed edges and use sorting for gradients...
		// Region growing here...

		CompareGrads::gradMatrix = grads;


		Matrix<int> tmp(grads.Rows(), grads.Columns(), 0);
		//Hashtable Marked = new Hashtable();
		bool greedy = false;
		int counter = 0;
		while(outsQ.size() > 0)
		{
			counter++;
			//Marked.Clear();
			int len = outsQ.size();
			//cout << "Length: " << len << endl;

			// copy queue to a list
			vector<Point> psA;
			for(int i=0; i<len; i++)
			{
				psA.push_back(outsQ.front());
				outsQ.pop();
			}
			std::sort( psA.begin( ), psA.end( ), CompareGrads::Compare );

			int marked = 0;
			for(int i=0; i<psA.size(); i++)
			{
				Point p = psA[i];
				if(labels(p.y,p.x) == 0)
				{
					vector<Point> A;


					for (int i=-1; i<=1; i++) 
					{
						for (int j=-1; j<=1; j++) 
						{
							if(i == 0 || j == 0) 
							{
								if(p.x+i>=0 && p.x+i<grads.Columns() && p.y+j>=0 && p.y+j<grads.Rows()) 
								{
									if(labels(p.y+j,p.x+i) != 0)
									{
										A.push_back(Point(p.x+i,p.y+j));
									}
								}
							}
						}
					}

					if(A.size() == 0)
					{
						outsQ.push(p);
					}
					else if(A.size() == 1)
					{
						Point p2 = A[0];
						if(suppressed(p.y,p.x) == 0 || greedy)
						{
							labels(p.y,p.x) = labels(p2.y,p2.x);
							marked++;
						}
						else
						{
							outsQ.push(p);
						}
					}
					else
					{
						Point pp = A[0];
						bool equal = true;
						for(int j=0;j<A.size();j++)
						{
							Point pp2 = A[j];
							if(labels(pp.y,pp.x) != labels(pp2.y,pp2.x))
							{
								equal = false;
							}
						}

						if(equal)
						{
							if(suppressed(p.y,p.x) == 0 || greedy)
							{
								labels(p.y,p.x) = labels(pp.y,pp.x);
								marked++;
							}
							else
							{
								outsQ.push(p);
							}
						}
					}
				}
				else
				{
					cout << "Something is wrong: A labeled point is added to the Queue!!!" << endl;
				}
			}


			if(marked == 0 && greedy)
			{
				break;
			}

			if(marked == 0)
			{
				greedy = true;
			}
		}
		






		for (int x=0; x<grads.Columns(); x++) 
		{
			for (int y=0; y<grads.Rows(); y++) 
			{
				if(labels(y,x) == 0)
				{
					edges(y,x) = 1;
				}
			}
		}

		cout << "Extracting boundaries finished!" << endl;

		return edges;


	
	
	}


	float Angle(float x1, float y1, float x2,  float y2)
	{
		float scalar = x1*x2+y1*y2;
		float m1 = sqrt(x1*x1+y1*y1);
		float m2 = sqrt(x2*x2+y2*y2);
		float ac = scalar/(m1*m2);
		ac = ac>1?1:ac;
		ac = ac<-1?-1:ac;
		return acos(ac)*180/PI;
	}


//The following code for RGB to Lab conversion is adapted from code 
//written by Yossi Rubner and Mark Ruzon. Following comments belong to them.
//Baris Sumengen 09/09/2005
//
//	Convert between RGB and CIE-Lab color spaces
//	Uses ITU-R recommendation BT.709 with D65 as reference white.
//	Yossi Rubner 2/24/98
//
//	Mark Ruzon 3/18/99 -- Added thresholds to truncate parts of the space
//	where changing a value has no visible effect on the color.


	void RGB2Lab(double R, double G, double B, double &L, double &a, double &b)
	{
		const double BLACK = 20;
		const double YELLOW = 70;
		double X, Y, Z, fX, fY, fZ;

		X = 0.412453*R + 0.357580*G + 0.180423*B;
		Y = 0.212671*R + 0.715160*G + 0.072169*B;
		Z = 0.019334*R + 0.119193*G + 0.950227*B;

		X /= (255 * 0.950456);
		Y /=  255;
		Z /= (255 * 1.088754);

		if(Y > 0.008856)
		{
			fY = pow(Y, 1.0/3.0);
			L = 116.0*fY - 16.0;
		}
		else
		{
			fY = 7.787*Y + 16.0/116.0;
			L = 903.3*Y;
		}

		if(X > 0.008856)
		{
			fX = pow(X, 1.0/3.0);
		}
		else
		{
			fX = 7.787*X + 16.0/116.0;
		}

		if(Z > 0.008856)
		{
			fZ = pow(Z, 1.0/3.0);
		}
		else
		{
			fZ = 7.787*Z + 16.0/116.0;
		}

		a = 500.0*(fX - fY);
		b = 200.0*(fY - fZ);

		if(L < BLACK) 
		{
			a *= exp((L - BLACK) / (BLACK / 4));
			b *= exp((L - BLACK) / (BLACK / 4));
			L = BLACK;
		}
		if(b > YELLOW)
		{
			b = YELLOW;
		}
	}


	void Lab2RGB(double L, double a, double b, double &R, double &G, double &B)
	{
		const double BLACK = 20;
		const double YELLOW = 70;
		double X, Y, Z, fX, fY, fZ;
		double RR, GG, BB;

		fY = pow((L + 16.0) / 116.0, 3.0);
		if(fY < 0.008856)
		{
			fY = L / 903.3;
		}
		Y = fY;

		if(fY > 0.008856)
		{
			fY = pow(fY, 1.0/3.0);
		}
		else
		{
			fY = 7.787 * fY + 16.0/116.0;
		}

		fX = a / 500.0 + fY;      
		if(fX > 0.206893)
		{
			X = pow(fX, 3.0);
		}
		else
		{
			X = (fX - 16.0/116.0) / 7.787;
		}

		fZ = fY - b /200.0;      
		if(fZ > 0.206893)
		{
			Z = pow(fZ, 3.0);
		}
		else
		{
			Z = (fZ - 16.0/116.0) / 7.787;
		}

		X *= (0.950456 * 255);
		Y *=             255;
		Z *= (1.088754 * 255);

		RR =  3.240479*X - 1.537150*Y - 0.498535*Z;
		GG = -0.969256*X + 1.875992*Y + 0.041556*Z;
		BB =  0.055648*X - 0.204043*Y + 1.057311*Z;

		R = (double)(RR < 0 ? 0 : RR > 255 ? 255 : RR);
		G = (double)(GG < 0 ? 0 : GG > 255 ? 255 : GG);
		B = (double)(BB < 0 ? 0 : BB > 255 ? 255 : BB);

	}


	MatrixList<float> RGB2Lab(MatrixList<float> input)
	{
		if(input.Planes() != 3)
		{
			cerr << "Line: " << __LINE__ << " File: " << __FILE__ << endl;
			Utility::RunTimeError("Length of MatrixList should be 3 for an RGB image!");
		}
		MatrixList<float> tmp(3, input.Rows(),input.Columns());
		for(int i=0;i<input.Rows();i++)
		{
			for(int j=0;j<input.Columns();j++)
			{
				double L,a,b;
				L = a = b = 0;
				RGB2Lab(input[0].ElemNC(i,j),input[1].ElemNC(i,j),input[2].ElemNC(i,j), L, a, b);
				tmp[0].ElemNC(i,j) = (float)L;
				tmp[1].ElemNC(i,j) = (float)a;
				tmp[2].ElemNC(i,j) = (float)b;
			}
		}
		return tmp;

	}


	MatrixList<float> Lab2RGB(MatrixList<float> input)
	{
		if(input.Planes() != 3)
		{
			cerr << "Line: " << __LINE__ << " File: " << __FILE__ << endl;
			Utility::RunTimeError("Length of MatrixList should be 3 for an Lab image!");
		}
		MatrixList<float> tmp(3, input.Rows(),input.Columns());
		for(int i=0;i<input.Rows();i++)
		{
			for(int j=0;j<input.Columns();j++)
			{
				double R,G,B;
				R = G = B = 0;
				Lab2RGB(input[0].ElemNC(i,j),input[1].ElemNC(i,j),input[2].ElemNC(i,j), R, G, B);
				tmp[0].ElemNC(i,j) = (float)R;
				tmp[1].ElemNC(i,j) = (float)G;
				tmp[2].ElemNC(i,j) = (float)B;
			}
		}
		return tmp;

	}


	extern "C" float *poisson(float *tmp, int X_DIM, int Y_DIM);

	Matrix<int> SegmentEF(Matrix<float> &im, bool normalized, float initScale, float scaleJump, 
		float endScale, float angleLimit, float ratioLimit, float smoothWeighting, float stopError, int accuracy)
	{
		float diag = sqrt(float(im.Rows()*im.Rows() + im.Columns()*im.Columns()));
		float atomic = diag/1000.0;
		cout << "Unit scale: " << atomic << " pixels" << endl << endl;
		float scale = initScale*atomic;

		cout << "Flow field at scale: " << scale << endl;
		MatrixList<float> flow = EdgeflowVectorField(im, 8, scale, scale, 1.0f, normalized);
		endScale = endScale*atomic;
		scaleJump = scaleJump*atomic;
		scale += scaleJump;
		while( scale <=  endScale)
		{
			Matrix<float> efMag = Sqrt(flow(0)*flow(0)+flow(1)*flow(1));
			float magLimit = Maximum(efMag(":"))/ratioLimit;

			cout << "Flow field at scale: " << scale << " pixels" << endl;
			MatrixList<float> tempflow = EdgeflowVectorField(im, 4, scale, scale, 1.0f, normalized);
			Matrix<float> tempMag = Sqrt(tempflow(0)*tempflow(0)+tempflow(1)*tempflow(1));

			for (int i=0; i<flow.Rows(); i++) {
				for (int j=0; j<flow.Columns(); j++) {
					if(efMag(i,j) < magLimit)
					{
						flow(0)(i,j) = tempflow(0)(i,j);
						flow(1)(i,j) = tempflow(1)(i,j);
					}
					else if( Angle(flow(0)(i,j),flow(1)(i,j),tempflow(0)(i,j),tempflow(1)(i,j)) < angleLimit )
					{
						flow(0)(i,j) += tempflow(0)(i,j);
						flow(1)(i,j) += tempflow(1)(i,j);
					}
				}
			}
			scale += scaleJump;
		}


		Matrix<float> BB = Divergence(flow(0), flow(1));
		float *tmp = poisson((-BB).Data(), BB.Columns(), BB.Rows());
		Matrix<float> CC(BB.Rows(), BB.Columns());
		for (int j=0; j<CC.Rows(); j++) {
			for (int i=0; i<CC.Columns(); i++) {
				CC(j,i) = tmp[i+j*CC.Columns()];
			}
		}
		ImWrite(CC, "edgefunction.png");
		float min = Minimum(CC(":"));
		float max = Maximum(CC(":"));
		CC -= min;
		CC *= 2.0f/(max-min);
		flow(0) *= 40.0f/(max-min);
		flow(1) *= 40.0f/(max-min);

		Matrix<float> b(im.Rows()+6, im.Columns()+6,0);
		b(3,im.Rows()+2,3,im.Columns()+2) = smoothWeighting*CC;

		Matrix<float> u(im.Rows()+6, im.Columns()+6,0);
		u(3,im.Rows()+2,3,im.Columns()+2) = flow(0);

		Matrix<float> v(im.Rows()+6, im.Columns()+6,0);
		v(3,im.Rows()+2,3,im.Columns()+2) = flow(1);

		Matrix<float> phi(im.Rows()+6, im.Columns()+6);
		phi(3,im.Rows()+2,3,im.Columns()+2) = im;

		Matrix<float> delta(im.Rows()+6, im.Columns()+6);
		Matrix<float> delta2(im.Rows()+6, im.Columns()+6);
		Matrix<float> old = im.Clone();

		PerfTimer pt200;
		pt200.Tic();
		float oldChange = 1e10;
		int k=0;
		while(1)
		{
			ExtendConst2D(phi,3);
			Evolve2DKappa(phi, 1, 1, 1, 1, b, delta);
			switch( accuracy )
			{
				case 1:
					Evolve2DVectorENO1(phi, 1, 1, u, v, delta2);
					break;
				case 2:
					Evolve2DVectorENO2(phi, 1, 1, u, v, delta2);
					break;
				case 3:
					Evolve2DVectorENO3(phi, 1, 1, u, v, delta2);
					break;
				case 5:
					Evolve2DVectorWENO(phi, 1, 1, u, v, delta2);
					break;
				default:
					cerr << "Line: " << __LINE__ << " File: " << __FILE__ << endl;
					Utility::RunTimeError("accuracy parameter can only be 1, 2, 3, or 5!");
			}

			float dt = Getdt2DVectorKappa(0.95f, 1, 1, 1, 1, u, v, b);
			AddPhi2D(phi,delta,dt);
			SubtractPhi2D(phi,delta2,dt);
			if(k%200 == 199 ){
				cout << "EF: " << k+1 << endl;
				pt200.Toc();
				char str[100];
				sprintf(str, "phi_%03d.bmp", k);
				for(int i=0; i<phi.Numel(); i++)
				{
					if(phi(i) < 0)
					{
						phi(i) = 0;
					}
					if(phi(i) > 255)
					{
						phi(i) = 255;
					}
				}
				Matrix<float> diffused = phi.Slice(3,im.Rows()+2,3,im.Columns()+2);
				//	ImWrite(diffused, str);
				float change = Sum(Vector<float>(Abs(old-diffused)))/im.Numel();
				cout << "Error: "  << change << endl << endl;
				if(change < stopError || k > 10000)
				{
					break;
				}
				if(change > oldChange)
				{

					Utility::Warning("!!!!! Instaability detected during diffusion... Exiting diffusion stage!");
					break;
				}
				oldChange = change;
				old = diffused.Clone();
				pt200.Tic();
			}
			k++;
		}

		phi = phi.Slice(3,im.Rows()+2,3,im.Columns()+2);
		for(int i=0; i<phi.Numel(); i++)
		{
			if(phi(i) < 0)
			{
				phi(i) = 0;
			}
			if(phi(i) > 255)
			{
				phi(i) = 255;
			}
		}

		ImWrite(phi, "diffused.png");

		Matrix<float> gx = FilterFDGaussian2D(phi, atomic, 0);
		Matrix<float> gy = FilterFDGaussian2D(phi, atomic, 90);
		Matrix<float> gradIm = Sqrt(SQR(gx) + SQR(gy));
		Matrix<float> gradSupp = NonMaximaSuppress(gradIm, gx, gy);
		Matrix<int> edges = GetEgdes(gradIm, gradIm > 1, gradSupp > 1);

		return edges;
	}




	Matrix<int> SegmentEF(MatrixList<float> &im, bool normalized, float initScale, float scaleJump, 
		float endScale, float angleLimit, float ratioLimit, float smoothWeighting, float stopError, int accuracy)
	{
		float diag = sqrt(float(im.Rows()*im.Rows() + im.Columns()*im.Columns()));
		float atomic = diag/1000;
		cout << "Unit scale: " << atomic << " pixels" << endl << endl;
		float scale = initScale*atomic;

		cout << "Flow field at scale: " << scale << " pixels";
		PerfTimer pt;
		pt.Tic();
		MatrixList<float> flow = EdgeflowVectorField(im, 8, scale, scale, 1.0f, normalized);
		double el = pt.Toc(false);
		cout << " (" << el << " seconds)" << endl;
		endScale = endScale*atomic;
		scaleJump = scaleJump*atomic;
		scale += scaleJump;
		while( scale <=  endScale)
		{
			Matrix<float> efMag = Sqrt(flow(0)*flow(0)+flow(1)*flow(1));
			float magLimit = Maximum(efMag(":"))/ratioLimit;

			cout << "Flow field at scale: " << scale << " pixels";

			pt.Tic();
			MatrixList<float> tempflow = EdgeflowVectorField(im, 4, scale, scale, 1.0f, normalized);
			double el = pt.Toc(false);
			cout << " (" << el << " seconds)" << endl;

			Matrix<float> tempMag = Sqrt(tempflow(0)*tempflow(0)+tempflow(1)*tempflow(1));

			for (int i=0; i<flow.Rows(); i++) {
				for (int j=0; j<flow.Columns(); j++) {
					if(efMag(i,j) < magLimit)
					{
						flow(0)(i,j) = tempflow(0)(i,j);
						flow(1)(i,j) = tempflow(1)(i,j);
					}
					else if( Angle(flow(0)(i,j),flow(1)(i,j),tempflow(0)(i,j),tempflow(1)(i,j)) < angleLimit )
					{
						flow(0)(i,j) += tempflow(0)(i,j);
						flow(1)(i,j) += tempflow(1)(i,j);
					}
				}
			}
			scale += scaleJump;
		}

		cout << endl;

		Matrix<float> BB = Divergence(flow(0), flow(1));
		float *tmp = poisson((-BB).Data(), BB.Columns(), BB.Rows());
		Matrix<float> CC(BB.Rows(), BB.Columns());
		for (int j=0; j<CC.Rows(); j++) {
			for (int i=0; i<CC.Columns(); i++) {
				CC(j,i) = tmp[i+j*CC.Columns()];
			}
		}
		ImWrite(CC, "edgefunction.png");
		float min = Minimum(CC(":"));
		float max = Maximum(CC(":"));
		CC -= min;
		CC *= 2.0f/(max-min);
		flow(0) *= 40.0f/(max-min);
		flow(1) *= 40.0f/(max-min);

		Matrix<float> b(im.Rows()+6, im.Columns()+6,0);
		b(3,im.Rows()+2,3,im.Columns()+2) = smoothWeighting*CC;

		Matrix<float> u(im.Rows()+6, im.Columns()+6,0);
		u(3,im.Rows()+2,3,im.Columns()+2) = flow(0);

		Matrix<float> v(im.Rows()+6, im.Columns()+6,0);
		v(3,im.Rows()+2,3,im.Columns()+2) = flow(1);

		Vector< Matrix<float> > phis(3);
		phis[0] = Matrix<float>(im.Rows()+6, im.Columns()+6);
		phis[0](3,im.Rows()+2,3,im.Columns()+2) = im[0];

		phis[1] = Matrix<float>(im.Rows()+6, im.Columns()+6);
		phis[1](3,im.Rows()+2,3,im.Columns()+2) = im[1];

		phis[2] = Matrix<float>(im.Rows()+6, im.Columns()+6);
		phis[2](3,im.Rows()+2,3,im.Columns()+2) = im[2];

		Matrix<float> delta(im.Rows()+6, im.Columns()+6);
		Matrix<float> delta2(im.Rows()+6, im.Columns()+6);

		Matrix<int> limits = "[0 255; -100 100; -100 100]";
		//cout << limits;

		for(int p=0;p<3;p++)
		{
			pt.Tic();
			int k=0;
			float oldChange = 1e10;
			Matrix<float> old = im[p].Clone();
			Matrix<float> phi = phis[p];
			while(1)
			{
				ExtendConst2D(phi,3);
				Evolve2DKappa(phi, 1, 1, 1, 1, b, delta);
				switch( accuracy )
				{
					case 1:
						Evolve2DVectorENO1(phi, 1, 1, u, v, delta2);
						break;
					case 2:
						Evolve2DVectorENO2(phi, 1, 1, u, v, delta2);
						break;
					case 3:
						Evolve2DVectorENO3(phi, 1, 1, u, v, delta2);
						break;
					case 5:
						Evolve2DVectorWENO(phi, 1, 1, u, v, delta2);
						break;
					default:
						cerr << "Line: " << __LINE__ << " File: " << __FILE__ << endl;
						Utility::RunTimeError("accuracy parameter can only be 1, 2, 3, or 5!");
				}

				float dt = Getdt2DVectorKappa(0.95f, 1, 1, 1, 1, u, v, b);
				AddPhi2D(phi,delta,dt);
				SubtractPhi2D(phi,delta2,dt);
				if(k%200 == 199 ){
					cout << "Diffusion: " << k+1 << " iterations ";
					double elapsed = pt.Toc(false);
					char str[100];
					sprintf(str, "phi_%03d.bmp", k);
					for(int i=0; i<phi.Numel(); i++)
					{
						if(phi(i) < limits(p,0))
						{
							phi(i) = limits(p,0);
						}
						if(phi(i) > limits(p,1))
						{
							phi(i) = limits(p,1);
						}
					}
					Matrix<float> diffused = phi.Slice(3,im.Rows()+2,3,im.Columns()+2);
					//	ImWrite(diffused, str);
					float change = Sum(Vector<float>(Abs(old-diffused)))/old.Numel();
					cout << "(Err: "  << change << ") (" << elapsed << "seconds)" << endl;
					if(change < stopError || k > 10000)
					{
						break;
					}
					if(change > oldChange)
					{

						Utility::Warning("!!!!! Instaability detected during diffusion... Finishing this plane!");
						break;
					}
					oldChange = change;
					old = diffused.Clone();
					pt.Tic();
				}
				k++;
			}
			cout << "!!!!!!! Image Plane " << p+1 << " is finished diffusing" << endl << endl;
			phis[p] = phis[p].Slice(3,im.Rows()+2,3,im.Columns()+2);
			for(int i=0; i<phis[p].Numel(); i++)
			{
				if(phis[p](i) < limits(p,0))
				{
					phis[p](i) = limits(p,0);
				}
				if(phis[p](i) > limits(p,1))
				{
					phis[p](i) = limits(p,1);
				}
			}
		}

		MatrixList<float> diffused(phis[0], phis[1], phis[2]);
		ImWrite(Lab2RGB(diffused), "diffused.png");

		Matrix<float> gx0;
		Matrix<float> gy0;
		Matrix<float> gx1;
		Matrix<float> gy1;
		Matrix<float> gx2;
		Matrix<float> gy2;

		//if(0)
		//{
		//	gx0 = FilterFDGaussian2D(phis[0], atomic, 0);
		//	gy0 = FilterFDGaussian2D(phis[0], atomic, 90);
		//	gx1 = FilterFDGaussian2D(phis[1], atomic, 0);
		//	gy1 = FilterFDGaussian2D(phis[1], atomic, 90);
		//	gx2 = FilterFDGaussian2D(phis[2], atomic, 0);
		//	gy2 = FilterFDGaussian2D(phis[2], atomic, 90);
		//}
		//else
		//{
			Gradient(phis[0], gx0, gy0);
			Gradient(phis[1], gx1, gy1);
			Gradient(phis[2], gx2, gy2);
		//}



		Matrix<float> aa = gx0*gx0 + gx1*gx1 + gx2*gx2;
		Matrix<float> bb = gx0*gy0 + gx1*gy1 + gx2*gy2;
		Matrix<float> cc = gy0*gy0 + gy1*gy1 + gy2*gy2;

		Matrix<float> eig = ((aa+cc)+Sqrt((aa-cc)*(aa-cc)+4*bb*bb))/2;
		Matrix<float> gradIm = Sqrt(eig);
		Matrix<float> theta = Atan2(eig-aa,bb);
		Matrix<float> gx = gradIm*Cos(theta);
		Matrix<float> gy = gradIm*Sin(theta);

		Matrix<float> gradSupp = NonMaximaSuppress(gradIm, gx, gy);
		Matrix<int> edges = GetEgdes(gradIm, gradIm > 1/3.0f, gradSupp > 1/3.0f);

		return edges;
	}





};